#ifndef _CONTROL_H_
#define _CONTROL_H_

#include <attitude_estimate.h>
#include <pwm.h>

#define EXPECT_ROLL_MAX_DEGREE 40.f  /* 滚转角期望最大值不超过40° */
#define EXPECT_PITCH_MAX_DEGREE 40.f /* 俯仰角期望最大值不超过40° */
#define EXPECT_YAW_MAX_DEGREE 180.f  /* 偏航角期望最大值不超过180° */

#define EXPECT_ROLL_ANGULAR_MAX_DEGREE 180.f  /* 偏航角速度期望最大值不超过120°/s */
#define EXPECT_PITCH_ANGULAR_MAX_DEGREE 180.f /* 偏航角速度期望最大值不超过120°/s */
#define EXPECT_YAW_ANGULAR_MAX_DEGREE 180.f   /* 偏航角速度期望最大值不超过120°/s */

#define ROLL_ERROR_KP_ROLL_ANGULAR 10.0f   /* 滚转角期望值和实测值的偏差映射到期望滚转角速度比例系数 */
#define PITCH_ERROR_KP_PITCH_ANGULAR 10.0f /* 俯仰角期望值和实测值的偏差映射到期望俯仰角速度比例系数 */
#define YAW_ERROR_KP_YAW_ANGULAR 10.0f     /* 偏航角期望值和实测值的偏差映射到期望偏航角速度比例系数 */

#define ATTITUDE_CONTROL_OUTPUT_MAX 2000.0f         /* 姿态控制时每个广义输出量最大值 */
#define ATTITUDE_CONTROL_OUTPUT_INTEGRAL_MAX 500.0f /* 姿态控制时每个广义输出量，其中积分项最大值 */
#define THROTTLE_MAX 3000.0f

#define ROLL_ANGULAR_KP 3.6f   /* 姿态内环控制：滚转角速度控制比例系数 */
#define ROLL_ANGULAR_KI 0.001f /* 姿态内环控制：滚转角速度控制积分系数 */
#define ROLL_ANGULAR_KD 50.0f  /* 姿态内环控制：滚转角速度控制微分系数 */

#define PITCH_ANGULAR_KP 3.6f   /* 姿态内环控制：俯仰角速度控制比例系数 */
#define PITCH_ANGULAR_KI 0.001f /* 姿态内环控制：俯仰角速度控制积分系数 */
#define PITCH_ANGULAR_KD 50.0f  /* 姿态内环控制：俯仰角速度控制微分系数 */

#define YAW_ANGULAR_KP 3.6f   /* 姿态内环控制：偏航角速度控制比例系数 */
#define YAW_ANGULAR_KI 0.001f /* 姿态内环控制：偏航角速度控制积分系数 */
#define YAW_ANGULAR_KD 50.0f  /* 姿态内环控制：偏航角速度控制微分系数 */

/* 控制模式 */
enum control_mode
{
    stablize,   /* 自稳模式 */
    fix_height, /* 定高模式 */
    fix_point   /* 定点模式 */
};

enum uav_status
{
    lock, /* 锁定状态 */
    arm   /* 解锁状态 */
};

typedef struct attitude_control
{
    float roll;   /* 俯仰方向 */
    float pitch;  /* 滚转方向 */
    float yaw;    /* 偏航方向 */
    float thrust; /* 油门方向 */
} attitude_control_t;

/// @brief 四旋翼是否成功解锁
/// @param
/// @return 返回无人机状态
enum uav_status get_uav_arm_status(void);

/// @brief 设置无人机的状态：解锁还是未解锁
/// @param armed 请参考 enum uav_status
/// @return
int set_uav_arm_status(enum uav_status new_status);

/// @brief 控制线程
/// @param
/// @return
int start_control_thread(void);

#endif